#include <eigen3/Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/highgui.hpp>
#include <opencv2/core/eigen.hpp>
// #include <tf2/LinearMath/Quaternion.h>

using namespace std;
using namespace cv;
using namespace Eigen;

vector<Vec4i> hierarchy;
void swap(int &, int &);
bool isRotationMatrix(Mat &);
// Vec3f rotationMatrixToEulerAngles(Mat &);
int main(int argc, char **argv)
{

    // Ptr<BackgroundSubtractor> pBackSub;
    ros::init(argc, argv, "basic_shapes");
    ros::NodeHandle n;
    ros::Rate r(1);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

    // Set our initial shape type to be a cube
    uint32_t shape = visualization_msgs::Marker::CUBE;

    // pBackSub = createBackgroundSubtractorMOG2();
    VideoCapture capture("/home/dale/cvrviz/water.avi");
    if (!capture.isOpened())
    {
        //error in opening the video input
        printf("could not load image..\n");
        return false;
    }
    Mat frame, hsv, mask, cornerpoint;
    while (ros::ok())
    {
        capture >> frame;
        if (frame.empty())
            break;

        cvtColor(frame, hsv, COLOR_BGR2HSV);
        inRange(hsv, Scalar(0, 43, 46), Scalar(26, 255, 255), mask);
        threshold(hsv, hsv, 70, 230, CV_THRESH_BINARY);
        // GaussianBlur(frame, frame, Size(11, 11), 3, 3);
        //闭操作联通+膨胀
        dilate(mask, mask, Mat(), Point(-1, -1), 14);
        erode(mask, mask, Mat(), Point(-1, -1), 19);
        dilate(mask, mask, Mat(), Point(-1, -1), 3);

        vector<vector<Point> > cont;
        findContours(mask, cont, CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
        Mat draw = Mat::zeros(mask.size(), CV_8UC3);
        // imshow("mask", mask);
        vector<RotatedRect> rect(cont.size());
        Mat savepoint[4];

        Point2f dot;
        vector<Point2f> savedots;

        if (cont.size() > 1)
        {
            for (int i = 0; i < cont.size(); i++)
            {
                //找出最小旋转矩形
                rect[i] = minAreaRect(cont[i]);
                //定义矩形的4个顶点
                Point2f vertices[4];
                // 排除上面灯管干扰，计算矩形的4个顶点
                if (rect[i].center.y > 80)
                {

                    rect[i].points(vertices);
                }
                // 两个边缘，八个点
                // for (int k = 0; k < 4; k++)
                //     cout << "第i个图" << i << "\t" << vertices[k] << endl;
                for (int j = 0; j < 4; j++)
                {
                    line(frame, vertices[j], vertices[(j + 1) % 4], Scalar(255, 255, 0), 0.03);
                    //提取corner points
                    if (vertices[j].x > 0 && abs(vertices[j].y - vertices[(j + 1) % 4].y) < 15)
                    {

                        dot = Point2f((vertices[j].x + vertices[(j + 1) % 4].x) / 2, (vertices[j].y + vertices[(j + 1) % 4].y) / 2);
                        savedots.push_back(dot);
                        // cout << dot << endl;
                        circle(draw, dot, 1, Scalar(255, 0, 255), 2, 8, 0);
                    }
                }
            }
            imshow("aa", frame);
            // imshow("draw", draw);
            //多甲板的话，可以对一个矩形临近点进行距离限定

            //冒泡算法
            int len = savedots.size();
            for (int i = 0; i < len - 1; ++i)
                for (int j = 0; j < len - 1 - i; ++j)
                    if (savedots[j].y < savedots[j + 1].y)
                    {
                        swap(savedots[j].x, savedots[j + 1].x);
                        swap(savedots[j].y, savedots[j + 1].y);
                    }
            if (savedots[0].x > savedots[1].x)
            {
                swap(savedots[0].x, savedots[1].x);
                swap(savedots[0].y, savedots[1].y);
            }
            if (savedots[2].x < savedots[3].x)
            {
                swap(savedots[2].x, savedots[3].x);
                swap(savedots[2].y, savedots[3].y);
            }
            for (int k = 0; k < 4; k++)
                cout << "第" << k << "\t" << savedots[k] << endl;

            //装甲参数
            vector<Point3f> objectPoints;
            objectPoints.push_back(Point3f(-67.5, 27.5, 0));
            objectPoints.push_back(Point3f(67.5, 27.5, 0));
            objectPoints.push_back(Point3f(67.5, -27.5, 0));
            objectPoints.push_back(Point3f(-67.5, -27.5, 0));
            //相机内参
            Mat cameraMatrix = (Mat_<double>(3, 3) << 1128.048344, 0., 339.421769, 0., 1127.052190, 236.535242, 0., 0., 1.);
            Mat distCoeffs = (Mat_<double>(5, 1) << -0.568429, 0.514592, -0.000126, 0.000500, 0.000000);
            //旋转向量和平移向量
            Mat rvec2, tvec2;
            solvePnP(objectPoints, savedots, cameraMatrix, distCoeffs, rvec2, tvec2);
            // cout << "rvec2 " << rvec2 << endl;
            // cout << "tvec2" << tvec2 << endl;
            Mat R1;
            Rodrigues(rvec2, R1);
            Matrix3d r1;
            cv2eigen(R1, r1);
            Quaterniond Q1(r1);
            double x = tvec2.at<double>(0, 0);
            double z = tvec2.at<double>(1, 0);
            double y = tvec2.at<double>(2, 0);

            // cout<<R1<<endl;
            // cout<<angle[0]<<endl;
            // for (int k = 0; k < 4; k++)
            // cout<<q[k]<<endl;
            visualization_msgs::Marker marker;
            // Set the frame ID and timestamp.  See the TF tutorials for information on these.
            marker.header.frame_id = "/my_frame";
            marker.header.stamp = ros::Time::now();
            // Set the namespace and id for this marker.  This serves to create a unique ID
            // Any marker sent with the same namespace and id will overwrite the old one
            marker.ns = "water";
            marker.id = 0;
            marker.type = shape;
            // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
            marker.action = visualization_msgs::Marker::ADD;
            // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
            // for (int k = 0; k < 3; k++)
            // cout<<"第"<<k<<"个"<< tvec2.at<double>(0,k)<<endl;
            marker.pose.position.x = x / 1000;
            marker.pose.position.y = y / 1000;
            marker.pose.position.z = -z / 1000;
            marker.pose.orientation.x = Q1.x();
            marker.pose.orientation.y = Q1.y();
            marker.pose.orientation.z = Q1.z();
            marker.pose.orientation.w = Q1.w();
            // Set the scale of the marker --  here means 1m on a side
            marker.scale.x = 0.135;
            marker.scale.y = 0.020;
            marker.scale.z = 0.055;
            // Set the color -- be sure to set alpha to something non-zero!
            marker.color.r = 0.0f;
            marker.color.g = 1.0f;
            marker.color.b = 0.0f;
            marker.color.a = 1.0;

            marker.lifetime = ros::Duration();
            // Publish the marker
            while (marker_pub.getNumSubscribers() < 1)
            {
                if (!ros::ok())
                {
                    return 0;
                }
                ROS_WARN_ONCE("Please create a subscriber to the marker");
            }

            marker_pub.publish(marker);
        }
        int keyboard = waitKey(30);
        if (keyboard == 'q' || keyboard == 27)
            break;
    }

    capture.release();
    return (0);
}

void swap(int &a, int &b)
{
    int tmp = 0;
    tmp = a;
    a = b;
    b = tmp;
}

// Checks if a matrix is a valid rotation matrix.
// bool isRotationMatrix(Mat &R)
// {
//     Mat Rt;
//     transpose(R, Rt);
//     Mat shouldBeIdentity = Rt * R;
//     Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

//     return norm(I, shouldBeIdentity) < 1e-6;
// }

// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
// Vec3f rotationMatrixToEulerAngles(Mat &R)
// {

//     assert(isRotationMatrix(R));

//     float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));

//     bool singular = sy < 1e-6; // If

//     float x, y, z;
//     if (!singular)
//     {
//         x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
//         y = atan2(-R.at<double>(2, 0), sy);
//         z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
//     }
//     else
//     {
//         x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
//         y = atan2(-R.at<double>(2, 0), sy);
//         z = 0;
//     }
//     return Vec3f(x, y, z);
// }
